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A  Fortran  program  was  developed  to  implement  a  Kalman  Filter  and  Fixed  Interval 
Smoothing  Algorithm  to  optimally  smooth  data  tracks  generated  by  the  short  base-line 
tracking  ranges  at  the  Naval  Torpedo  Station,  Keyport,  Washington.  The  program  is 
designed  to  run  on  a  personal  computer  and  requires  as  input  a  data  file  consisting  of 
X,  Y,  and  Z  position  coordinates  in  sequential  order.  Data  files  containing  the  filtered 
and  smoothed  estimates  are  generated  by  the  program.  This  algorithm  uses  a  second 
order  linear  model  to  predict  a  typical  target’s  dynamics.  The  program  listings  are  in¬ 
cluded  as  appendices. 

Several  runs  of  the  program  were  performed  using  actual  range  data  as  inputs.  Re¬ 
sults  indicate  that  the  program  effectively  reduces  random  noise,  thus  providing  very- 
smooth  target  tracks  which  closely  follow  the  raw  data.  Tracks  containing  data  gener¬ 
ated  in  an  overlap  region  where  one  array  hands  off  the  target  to  the  next  array  are 
highlighted.  The  effects  of  varying  the  magnitude  of  the  excitation  matri.\  Q(k)  are  also 
explored. 

This  program  is  seen  as  a  valuable  post-data  analysis  tool  for  the  current  tracking 
range  data,  fn  addition,  it  can  easily  be  modified  to  provide  improved  real  time,  on  line 
tracking  using  the  Kalman  Filter  portion  of  the  algorithm  alone. 
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I.  INTRODUCTION 


The  short  base-line  array  tracking  ranges  located  near  the  Naval  Torpedo  Station 
at  Keyport,  Washington,  are  used  to  conduct  torpedo  testing  in  support  of  torpedo  de¬ 
velopment  projects.  The  tracking  system  in  use  on  these  ranges  consists  of  a  series  of 
short  base-line  arrays  which  can  independently  track  several  subsurface  targets  by  re¬ 
laying  received  signals  from  each  target  to  a  tracking  computer.  The  computer  processes 
the  received  array  data  and  calculates  target  tracks  in  X,Y,and  Z  coordinates  for  display. 
The  arrays  are  distributed  over  the  range  so  that  full  coverage  of  the  range  is  achieved, 
and  as  a  result  there  are  several  regions  on  the  range  where  the  arrays  overlap  in  their 
coverage.  Figure  I  on  page  2  is  an  illustration  of  this  arrangement  [Ref  1;  p.  199]. 
Handoff  is  a  term  used  to  describe  when  the  tracking  information  on  a  target  supplied 
to  the  tracking  computer  is  shifted  from  one  array  to  a  neighboring  array. 

Each  short  base-line  array  consists  of  four  omnidirectional  hydrophones  spaced  30 
feet  apart  on  orthogonal  booms  as  shown  in  Figure  2  on  page  3.  The  four  hydrophones 
are  all  connected  to  a  common  cable  which  feeds  the  received  signal  from  each 
hydrophcjne  to  the  tracking  computer.  The  computer  extracts  the  different  times  of  ar¬ 
rival  of  the  received  signals  and  calculates  X,Y,  and  Z  coordinates  based  on  the  following 
equations: 

X  =  -^iTc^-Tx^)  (1.1) 

Y  =  -^{Tc^-Ty^)  (1.2) 

Z  =  -^(rc‘-rz')  (1.3) 


where: 

V  =  speed  of  sound  in  water, 

D  =  hydrophone  separation  distance, 

Tc  =  tracking  signal  travel  time  from  target  to  hydrophone  C, 

Tx  =  tracking  signal  travel  time  from  target  to  hydrophone  X, 

Ty  =  tracking  signal  travel  time  from  target  to  hydrophone  Y,  and 
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Figure  1.  Short  Base-line  Array  Range  Configuration  -  Fig  2  From  Ref  1. 

Tz  =  tracking  signal  travel  time  from  target  to  hydrophone  Z. 

A  more  detailed  description  of  the  array  and  derivation  of  the  equations  above  can  be 
found  in  (Ref  2]. 

The  tracking  signal  used  on  these  ranges  is  a  150  watt  phase  shift  keying  (PSK)  pulse 
consisting  of  47  bits  (19  identification  bits  and  27  telemetry  bits)  and  lasting  approxi¬ 
mately  five  milliseconds.  Ihe  O's  and  I's  making  up  the  bit  stream  are  180  degrees  out 
of  phase.  Correlation  processing  techniques  are  used  to  validate  the  received  signal,  thus 
improving  the  signal  to  noise  ratio.  [Ref.  3:  p.  9J 

Figures  3  and  4  show  examples  of  a  typical  track  generated  by  this  system. 
Figure  3  on  page  4  is  the  track  in  X  vs.  Y  coordinates  generated  from  data  received  by 
a  single  array  while  Figure  4  on  page  5  shows  the  same  tracks  in  X  vs.  Z  coordinates. 
It  is  obvious  that  these  tracks  are  affected  by  random  noise.  Figures  5  and  6  show  ex¬ 
amples  of  a  track  generated  with  data  from  two  arrays  as  would  occur  in  an  overlap  re¬ 
gion.  Again,  the  X  vs.  Y  coordinates  and  X  vs.  Z  coordinates  format  is  used.  It  can 
be  seen  that  the  data  from  the  different  arrays  do  not  agree.  This  indicates  that  bias 
errors  are  also  present  in  the  current  tracking  system.  Tracking  accuracy  has  been  im¬ 
proved  by  use  of  the  PSK  pulse  because  its  sophistication  increases  the  received  signal 


Figure  2.  Short  Base-line  Array  Configuration. 
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Figure  3.  Typical  Track  Generated  By  A  Single  Array  (X  vs  Y). 


to  noise  ratio,  but  it  can  be  seen  from  the  tracks  that  a  great  deal  of  noise  remains.  1  he 
question  that  now  arises  is  how  these  target  tracks  can  be  improved. 

While  several  approaches  to  improving  the  tracking  accuracy  of  the  short  base-line 
array  ranges  are  possible,  the  effort  here  has  been  on  applying  a  Kalman  Filter  and 
Fixed  Interval  Smoothing  Algorithm  as  a  post  data  analysis  tool.  Quite  a  bit  of  work 
has  already  been  done  in  the  area  of  applying  Kalman  Filters  to  underwater  tracking 
over  the  years  (Refs.  4,  5,  6,  7].  More  recent  efforts  have  centered  around  applying  an 
optimal  smoothing  algorithm  to  the  underwater  tracking  problem  (Refs.  8,  9].  Most  of 
this  work  has  involved  attempts  to  filter  or  smooth  the  transit  times  prior  to  their  use 
in  the  tracking  equations  using  an  Extended  Kalman  Filter,  in  the  program  developed 
in  this  research,  a  Kalman  Filter  utilizing  a  second  order  linear  model  is  used  to  reduce 
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the  random  noise  in  the  already  calculated  X,  Y,  and  Z  position  coordinates.  1  he  opti¬ 
mal  Fixed  Interval  Smoothing  algorithm  is  then  used  to  improve  the  track  quality  even 
further.  This  method  is  used  because  it  lends  itself  more  readily  to  post  data  analysis. 
The  program  is  written  in  Fortran,  compiled  using  the  Microsoft,  Inc.  4.01  Optimal 
Compiler  and  run  on  an  IBM-AT  personal  computer.  Details  of  the  program  and  results 
obtained  will  now  be  discussed. 
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Figure  5.  Track  Generated  By  Tuo  Arrays  In  An  Overlap  Region  (X  vs  Y). 
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II.  KALMAN  FILTER 


The  Kalman  Filter  is  designed  to  remove  random  noise  from  the  estimate  of  the 
parameter  with  which  it  is  concerned  by  adding  a  weighted  error  term  to  the  observed 
parameter  being  estimated.  The  error  term  is  simply  the  difference  between  the  filter's 
prediction  of  the  parameter  and  the  actual  value  observed  at  a  particular  time.  The 
weighting  factor  is  influenced  by  the  magnitude  of  the  measurement  error  and  the 
covariance  of  error  between  the  estimate  and  the  observation.  The  covariance  of  error 
of  the  estimate  is  also  updated  based  on  previous  values  of  the  covariance  of  error. 
Many  treatments  of  the  derivation  and  application  of  Kalman  Filters  were  found  helpful 
in  preparing  this  report  [Refs.  10,  11,  and  12],  and  the  equations  used  in  this  application 
will  now  be  presented. 

The  equations  shown  below  were  obtained  directly  from  previous  work  on  this  sub¬ 
ject  [Ref  9:  p.  19).  Their  development  will  not  be  repeated  here  as  an  excellent  deriva¬ 
tion  of  these  equations  is  presented  in  [Ref  13:  pp.  176-182]. 


i(A- 1  A)  =  1  A  -  1)  G(A)C2(A)  -  £(A  I  a  -  1)] 

(2.1) 

i(A.f  l!A)  =  (/>i(A|A) 

(2.2) 

2(A  I  a  -  1)  =  H£{k  Ik-l) 

(2.3) 

G(A)  =  P(A  1  A  -  |  A  -  1)//^+  R]"' 

(2.4) 

P{k  +  l\k)^4>P{k\k)<t>^+Q{k) 

(2.5) 

P{k  1  A)  =  [/  -  Gik)H]P{k  1  A  -  1) 

(2.6) 

where; 

i  -  state  estimate  vector, 
i  -  observation  vector, 

H  -  measurement  matrix, 

<j)  -  state  transition  matrix, 

G  -  Kalman  gain  matrix, 

P  -  covariance  of  estimate  error  matrix, 


8 


R  •  covariance  of  measurement  error  matrix, 

Q  -  covariance  of  excitation  error  matrix,  and 
/  -  identity  matrix. 

In  this  application,  six  states  were  used;  namely,  the  x,  y,  and  z  positons  and  the 
corresponding  velocities,  x  ,y,  and  i.  The  model  for  the  system,  represented  by  the  state 
transition  matrix  0,  is  a  second  order  linear  model 

1  r  0  0  0  0 

0  1  0  0  0  0 

0  0  1  r  0  0 

0  0  0  1  0  0 

0  0  0  0  1  r 

0  0  0  0  0  1 

which  when  multiplied  by  the  state  matrix  £{k  |  k)  results  in  the  equations  of  motion 

x(k+l)^Hk)  +  x{k)T  (2.8) 

^ik+l)=Hk)-^kk)T  (2.9) 

z{k+l)  =  z{k)  +  z{k)T  (2.10) 

where  T  represents  the  time  in  seconds  between  samples.  Since  the  parameters  being 
estimated  are  already  expressed  in  a  linear  coordinate  system,  the  measurement  matrix 
H  necessarx’  in  this  case  need  only  extract  the  observ^able  states,  i.e.,  x,  y,  and  z.  The 
measurement  matrix  used  is: 

’l  0  0  0  0  o' 

//=  0  0  1  0  0  0  (2.11) 

_0  0  0  0  1  0_ 

The  values  of  Q(k)  and  R  chosen  play  an  important  role  in  the  performance  of  the 
filter.  The  covariance  of  excitation  error,  Q(k),  is  a  measure  of  how  confident  the  filter 
is  in  the  adequacy  of  the  system  model  together  with  how  strong  the  noise  affecting  the 
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actual  system  is  expected  to  be.  A  relatively  large  value  of  Q(k)  results  in  the  filter 
placing  more  emphasis  on  the  observation  and  less  on  the  predicted  value  when  updating 
the  estimate,  while  a  smaller  value  of  Q(k)  has  the  opposite  effect.  Therefore,  with  a 
larger  Q(k),  the  filter  output  should  be  noisier  but  better  able  to  handle  disturbances  not 
related  to  noise  such  as,  in  this  case,  actual  target  maneuvers.  On  the  other  hand,  the 
covariance  of  measurement  error  R  indicates  the  confidence  in  the  accuracy  of  the  data 
measurements  made.  It  turns  out  that  increasing  the  magnitude  of  R  in  effect  decreases 
the  gain  resulting  in  less  weight  being  given  to  the  difference  between  the  predicted  esti¬ 
mate  and  the  actually  observed  value.  This  results  in  less  attention  being  paid  to  the 
noise  observed,  which  makes  sense  if  it  is  assumed  that  the  measured  values  are  less  ac¬ 
curate.  [Ref  12:  p.  224] 

The  covariance  of  excitation  error  matrix  used  for  this  filter  application  is  as  follows: 


Q  = 


\V 


0 

0 

0 

0 


0 


f^w 


0 

0 

0 

0 


0 

-T"' 

0 

0 

0 


0 

0 

0 

f^w 

0 

0 


7^ 


w 


0 

0 

0 


T^W 


(2.12) 


where: 

W=ol  =  aj  =  al  =  Elww^2  (2.13) 

A  detailed  derivation  of  this  matrix  is  shown  in  [Ref  4:  pp.  36-42).  Since  the  excitation 
errors  are  assumed  to  be  uncorrelated,  only  the  diagonal  terms  of  this  matrix  are  non¬ 
zero.  The  value  of  W  is  set  by  the  user  while  running  the  program.  The  value  of  R 
chosen  is  25  square  feet.  This  is  a  conservative  value  based  on  range  accuracy  estimates 
reported  by  NUWES  [Ref  3:  p.  6). 
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Finally,  in  initializing  the  filter,  an  initial  value  for  the  covariance  of  estimate  error 
P(k,'k)  must  be  chosen.  This  value  affects  the  transient  response  of  the  filter  early  on, 
but  does  not  affect  the  steady  state  response  (Ref.  12;  p.  224].  For  this  filter,  P(k  k)  in¬ 
itially  is  set  at  one  million  square  feet.  Again,  uncorrelated  errors  were  assumed,  so,  only 
the  main  diagonal  terms  are  non-zero.  The  initial  value  of  P(k;k)  used  here  was  arrived 
at  mainly  through  trial  and  error. 

In  summarj’,  the  Kalman  Filter  is  a  linear  minimum  variance  estimator  whose  out¬ 
put  is  nothing  more  than  the  conditional  mean  of  the  parameter  being  estimated  based 
on  the  observations  made.  A  more  detailed  description  of  the  actual  programs  can  be 
found  in  Appendix  A  and  Appendix  B. 

This  program  was  run  on  a  typical  data  file  obtained  from  information  provided  by 
the  Key-port  Range.  Figures  3  and  4  show-  the  raw  track  in  X  vs.  Y  and  X  vs.  Z  coor¬ 
dinates  respectively.  Figure  7  on  page  12  shows  the  Kalman  Filter  variance  of  the  esti¬ 
mate  of  the  X  coordinate  of  the  target's  track.  It  can  be  seen  that  the  variance  settles 
quickly  to  a  steady  state  value  of  about  five  square  feet.  The  added  uncertainty,  intro¬ 
duced  by  the  fact  that  periodically  samples  are  missed  by  the  range  tracking  system,  is 
evident  in  that  the  values  of  P(k  k)  do  not  settle  to  a  constant  value,  but  this  uncertainty 
is  absorbed  by  the  filter  and  causes  no  problem  in  the  filter's  performance. 

Figure  8  on  page  13  shows  the  Kalman  filtered  track  for  the  coordinates  X  vs.  Y. 
Clearly,  the  filtered  track  is  much  smoother  than  the  raw  data  and  follows  the  raw  track 
closely  after  the  filter  settles  out.  However,  the  filter  tracks  a  little  low  after  the  target 
appears  to  make  a  sharp  maneuver  to  its  left  because  the  value  of  Q(k)  is  small.  In  other 
words,  a  small  value  of  Q{k)  does  not  allow  the  filter  to  follow  maneuvers  well,  and  this 
raw  data  set  looks  like  it  is  maneuvering  initially.  Figure  9  on  page  14  is  the  same  data 
as  Figure  8  on  page  13  except  that  the  Z  coordinates  are  plotted  against  the  X  coordi¬ 
nates.  This  plot  does  not  exhibit  the  same  track  off  behavior  because  no  maneuvers  are 
apparent  and,  in  fact,  a  great  deal  of  noise  reduction  is  shown. 

Another  run  of  the  program  was  made  using  a  value  of  Q(k)  increased  by  a  factor 
of  100.  Figure  10  on  page  15  shows  the  variance  of  x  and,  as  expected,  it  follows  the 
same  pattern  but  settles  at  a  higher  “steady  state”  value  of  approximately  1 1  square  feet. 
Also,  the  jumps  in  P(k'k)  due  to  the  missed  samples  are  more  pronounced.  Figure  1 1 
on  page  16  is  a  plot  of  X  vs.  Y  and,  when  compared  to  Figure  8  on  page  13,  clearly 
exhibits  the  expected  behavior.  The  filtered  track  is  much  noisier,  but  it  does  not  track 
off  as  before.  Figure  12  on  page  17  displays  X  vs.  Z  and  again  is  a  noisier  track  which 
follows  the  raw  data  more  closely. 
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Figure  7.  Kalman  Filter  Variance  Of  X  Estimate  (Small  Q(k)). 

The  Kalman  F'ilter  routine  designed  for  this  application  provides  the  user  with  a  lil- 
tered  set  of  data  points  wliich  are  an  improvement  over  the  actual  raw  data  available  in 
that,  as  expected,  much  of  the  random  noise  which  tends  to  corrupt  the  raw  data  has 
been  eliminated.  Proper  selection  of  the  R  and  Q(k)  matricies  and  the  initial  value  of 
P(k,'k)  ensures  that  the  filter  is  receptive  to  target  maneuvers  while  at  the  same  time 
significantly  reducing  the  random  noise.  The  user  must  decide  on  the  proper  balance  to 
fit  his  needs  based  on  known  information  such  as  the  expected  maneuverability  of  the 
target  and  the  magnitude  of  the  expected  measurement  errors.  In  addition,  this  filtered 
data  is  now*  ready  to  be  processed  by  the  Fixed  Interval  Smoothing  Routine. 
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Figure  8,  Kalman  Filtered  Track  -  X  vs.  Y  (Small  Q(k)). 


13 


DOWNRANGE  (FT)  xl( 


Figure  9.  Kalman  Filtered  Track  -  X  vs.  Z  (Small  Q(k)). 
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Figure  11.  Kalman  Filtered  Track  -  X  vs.  Y  (Large  Q(k)). 
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III.  FIXED  INTERVAL  SMOOTHING  ALGORITHM. 

The  Fixed  Interv’al  Smoothing  Algorithm  is  one  of  the  three  algorithms  designed  to 
improve  upon  the  Kalman  Filter's  results  by  taking  into  account  data  which  was  not 
available  when  each  Kalman  Filter  estimate  was  made  and  updating  each  previous  esti¬ 
mate  accordingly.  The  Fixed  Point  and  Fixed  Lag  Smoothing  algorithms,  while  very 
similar  to  the  Fixed  Interval  Smoothing  routine,  differ  in  the  way  they  include  this  ad¬ 
ditional  data  into  each  estimate.  The  Fixed  Interval  Smoothing  algorithm  recalculates 
each  estimate  generated  by  its  associated  Kalman  Filter  based  on  information  obtained 
over  the  entire  interval  of  data  being  analyzed.  In  this  sense,  it  is  useful  only  as  a  post 
data  analysis  tool,  since  the  entire  set  of  data  over  the  given  interval  must  be  known,  and 
Kalman  Filter  estimates  and  covariance  of  error  between  estimates  and  observations 
must  be  generated  previously.  In  implementing  this  algorithm,  a  system  of  recursive 
equations  which  operate  backwards  in  time  from  the  last  data  point  to  the  first  data 
point  are  used. 

The  equations  for  the  Fixed  Interval  Smoothing  algorithm  used  in  this  application 
were  obtained  from  [Ref  9:  p.  20].  Several  sources  were  beneficial  in  understanding 
these  equations  (Refs.  10,  11,  and  14  ]  and  [Ref  13;  pp.  216-225]  provides  an  e.xcellent 
derivation  of  then..  Therefore,  these  derivations  will  not  be  repeated  here.  The 
equations  of  interest  are: 

i(A- 1  X)  =  £(k  I  k)  -I-  -t-  1  I  A-)  _  +  1  I  ^)] 

/f(A)  =  P(k  i  k)<p'^P(k  1  I  A)"' 

Pik  t  Al  =  P{k  I  A)  A{k)lPik  -t-  1|  :V)  -  P{k  +  1  |  A)],4(A)^ 

where: 

i(A  I  X)  -  smoothed  estimate  at  sample  k, 

P{k  I  X)  -  smoothed  covariance  of  estimate  error  at  sample  k, 

A{^k)  -  smoothing  algorithm  gain  matrix,  and 
i(A  I  A)  and  P{k  |  A)  -  values  stored  by  the  Kalman  Filter  routine. 

It  can  be  seen  from  the  above  equations  that  the  estimate  generated  by  the  Fixed 
Interval  Smoothing  algorithm  is  simply  the  Kalman  Filter  estimate  adjusted  by  a 


(3.1) 

(3.2) 

(3.3) 
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weighted  error  term.  The  error  term  is  the  difTerence  between  the  smoothed  estimate 
calculated  for  the  previous  data  point  (which  is  actually  the  next  sequential  data  point 
in  the  file  in  real  time),  and  the  predicted  value  of  the  corresponding  parameter  gener¬ 
ated  by  the  Kalman  Filter.  The  gain  matrix  A(k)  is  dependent  on  the  covariance  of  error 
between  estimate  and  observation  generated  by  the  Kalman  Filter,  namely,  P(k;k)  and 
P(k+  I,'k).  The  smoothed  value  of  the  covariance  of  error  P(k/\),  while  having  no  im¬ 
pact  on  the  smoothed  estimate  x{k  j  A^) ,  provides  a  measure  of  how  well  the  smoothing 
filter  is  working.  In  general,  P(k,'X)  should  be  less  than  P(k;k)  except  at  the  Nth  point 
where  they  should  equal  one  another.  As  the  smoothing  filter  moves  backward  in  time, 
it  adjusts  the  original  Kalman  Filter  estimate  depending  on  the  smoothed  estimate's 
agreement  with  the  predicted  value  for  the  previous  point  operated  on,  and  on  the  con¬ 
fidence  level  of  the  Kalman  Filter  in  its  own  solution  as  indicated  by  the  values  of  P(k  k) 
and  P(k+  I  k).  Simply  stated,  if  more  uncertainty  exists  in  the  Kalman  Filter  Solution, 
more  weight  is  given  to  the  difference  between  previous  smoothed  estimates  and  pre¬ 
dicted  values  of  the  parameter  in  computing  the  current  smoothed  estimate. 

The  Fixed  Interval  Smoothing  portion  of  the  program  listed  in  Appendix  A  imple¬ 
ments  the  equations  shown  above.  As  with  the  Kalman  Filter  portion  of  the  program 
described  in  the  previous  chapter,  there  is  no  provision  for  the  resultant  data  to  be  dis¬ 
played  graphically  by  this  program  itself  because  the  size  of  the  data  files  involved  ex¬ 
ceeds  the  capabilities  of  the  plotting  routines  available.  However,  graphical  results  are 
easily  obtained  from  the  data  files  output  by  the  program  using  Matlab.  Details  of 
Matlab  plotting  capabilities  and  descriptions  of  the  commands  used  to  generate  the  plots 
shown  can  be  found  in  [Ref  15]. 

Results  of  the  smoothing  algorithm  were  obtained  using  the  Kalman  Filter  results 
presented  in  the  previous  chapter  as  inputs  to  the  smoothing  loop  of  the  program.  As 
in  chapter  2,  data  is  presented  in  the  X  vs.  Y  plane  and  the  X  vs.  Z  plane,  and  cases  were 
run  for  both  values  of  Q(k). 

Figure  13  on  page  20  shows  the  variance  of  the  x  estimate  associated  with  the 
smoothing  routine  together  with  the  variance  of  x  achieved  with  the  Kalman  Filter.  As 
expected,  the  value  of  the  smoothed  variance  is  a  definite  improvement  over  the  Kalman 
Filter  result,  and  in  fact  settles  to  an  average  value  that  is  less  than  1.5  square  feet. 
Figure  14  on  page  21  is  the  smoothed  version  of  Figure  8  superimposed  on  the  raw  X 
vs.  Y  track  for  the  case  where  Q(k)  is  small  and  shows  a  smooth  track  ’vhich  follows  the 
raw  track  closely.  The  divergence  seen  initially  in  the  filtered  results  for  this  case  has 
been  compensated  for,  however,  a  close  look  at  the  smoothed  results  shows  some  di- 
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Figure  13.  Kalman  Filtered  &  Smoothed  Variances  Of  X  (Small  Q(k)). 


vergence  still  present  in  the  first  portion  of  the  track.  Figure  15  on  page  22  is  the 
smoothed  version  of  Figure  9  superimposed  on  its  raw  data,  and  again  a  significant  im¬ 
provement  is  demonstrated. 

1  he  Kalman  Filter  results  of  the  case  where  a  large  Q(k)  was  used  were  also  treated 
with  the  smoothing  algorithm.  Figure  16  on  page  23  is  the  smoothed  plot  of  the  vari¬ 
ance  of  the  X  estimate  together  with  the  variance  of  x  shown  in  Figure  10.  This  plot 
indicates  that  the  output  is  much  more  influenced  by  noise  and  sample  uncertainty  due 
to  the  higher  sensitivity  when  compared  to  the  small  Q(k)  case,  but  is  a  great  improve¬ 
ment  over  the  Kalman  Filter  output.  Mere  the  variance  settles  to  an  average  value  of 
approximately  five  square  feet.  I'igurc  17  on  page  24  shows  the  smoothed  version  of 
Figure  1 1  superimposed  on  its  raw  track.  This  smoothed  track  greatly  improves  on  the 
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Figure  14.  Ra«  &  Smoothed  Tracks  -  X  vs.  Y  (Small  Q(k)). 


filtered  track  while  showing  none  of  the  divergence  from  the  raw  track  which  was  seen 
in  the  small  Q(k)  case.  Figure  18  on  page  25  is  the  raw  and  smoothed  X  vs.  Z  tracks 
for  the  large  Q(k)  case  and,  when  compared  to  the  Kalman  Filter  results  shown  in  Figure 
12,  clearly  illustrates  the  e.xcellent  performance  of  the  smoothing  routine. 

These  results  show  that  the  smoothing  algorithm  dramatically  improves  upon  the 
results  obtained  with  the  Kalman  Filter  alone.  This  intuitively  makes  sense  because  the 
estimates  are  now  based  on  the  behavior  of  the  entire  data  set  instead  of  on  just  what 
is  known  about  the  data  at  the  time  the  estimate  is  made.  The  smoothed  results  depend 
indirectly  upon  the  values  chosen  for  Q(k)  and  P(k,'k)  initially  and  can  even  compensate 
somewhat  for  poor  choices  of  the  parameters  in  the  initialization  of  the  Kalman  Filter. 
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Figure  15.  Raw  &  Smoothed  Tracks  -  X  vs,  Z  (Small  Q(k). 

These  results  demonstrate  that  the  Kalman  Filter  Smoothijig  Algorithm  employed  here 
is  an  excellent  post-data  analysis  tool. 
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Figure  18.  R.i>v  &  Suiootlied  Tracks  -  X  vs.  Z  (Large  Q(k)). 


IV.  SOLVING  THE  ARRAY  HANDOFF  PROBLEM. 

As  discussed  in  the  introduction,  one  of  the  major  problems  with  the  tracks  gener¬ 
ated  by  the  current  Keyport  short  base-line  tracking  array  system  is  the  presence  of 
discontinuities  in  array  overlap  regions.  These  discontinuities  in  the  target  track  caused 
by  differing  bias  errors  present  in  each  array's  solution  complicate  the  post  data  analysis 
so  crucial  to  torpedo  testing  and  evaluation.  It  can  now  be  demonstrated  that  the  Fixed 
Interval  Smoothing  Kalman  Filter  Algorithm,  as  implemented  by  the  program  presented 
here,  can  improve  the  overall  track  quality  immensely  in  the  overlap  regions. 

As  shown  in  previous  chapters,  the  Kalman  Filter  and  Fixed  Interval  Smoothing 
routines  effectively  remove  random  noise  from  the  generated  track  data  and  generate 
smooth  tracks  which  are  easy  to  see.  The  processing  of  track  data  from  two  arrays 
where  many  sample  times  have  two  different  values  simultaneously  is  handled  quite 
satisfactorily  by  the  algorithms.  The  data  is  treated  simply  as  two  distinct  samples  where 
no  time  has  elapsed  between  samples.  This  overlap  data  is  also  characterized  by  the  fact 
that  many  samples  are  missed  and  occasionally  relatively  long  periods  of  time  pass  be¬ 
tween  data  points.  It  will  be  seen  that  this  problem  is  also  handled  adequately  by  the 
algorithms.  Another  problem  experienced  \\ith  these  data  sets  was  that  they  were  too 
large  to  be  handled  by  the  personal  computer's  compiler.  This  problem  was  solved  by 
switching  to  the  Microsoft,  Inc.  4.01  Optimizing  Compiler.  This  compiler  not  only 
compiled  the  program  using  less  memor>’,  but  use  of  the  SLARGE  metacommand  al¬ 
lowed  the  larger  data  sets  to  be  processed.  Detailed  descriptions  of  this  compiler  and  the 
available  memory  models  can  be  found  in  [Ref  16].  Two  sets  of  overlap  data  were  an¬ 
alyzed  and  the  results  of  this  analysis  are  now  presented. 

Figure  19  on  page  27  is  a  plot  of  the  Kalman  Filter  variance  of  the  x  coordinate 
estimate  vs.  time,  together  with  the  smoothed  variance  of  the  x  estimate  for  the  first  set 
of  overlap  data  processed.  It  is  seen  that  the  filtered  variance  drops  quickly  to  a  steady 
state  value  of  about  6.5  square  feet,  and  then  jumps  back  up  erratically  over  the  last 
third  of  the  set.  The  thick  portion  of  the  filter's  variance  is  where  overlap  is  occurring 
because  here  two  values  of  the  parameter  are  available  for  each  time,  so,  two  steady  state 
values  of  the  variance  of  the  estimate  are  consistently  reached.  The  erratic  values  of  the 
filter's  variance  result  from  the  fact  that  over  this  region  the  sample  interval  varies  er¬ 
ratically  as  samples  were  missed,  so,  the  uncertainty  of  the  filter  is  also  affected.  The 


26 


Figure  19.  Kalman  Filtered  &  Smoothed  Variances  Of  X  (Arrays  1  &  11). 


smootliing  algorithm's  variance  of  the  x  estimate  is,  as  expected,  a  significant  improve¬ 
ment  over  the  filter's  performance  alone.  Note  that  the  overlap  region  now  settles  to  a 
single  steady  state  variance  of  about  2.5  square  feet,  and  that,  although  the  variance 
jumps  up  over  tlie  latter  portion  of  the  data  set  as  before,  the  values  are  again  much 
smaller  than  the  filtered  result.  Figure  20  on  page  28  displays  the  Kalman  filtered  track 
of  the  data  shown  in  Figure  5  for  the  X  vs.  Y  coordinates  case.  The  filtered  track  over¬ 
shoots  the  raw  track  initially,  indicating  that  better  tuning  may  be  required  as  discussed 
earlier,  but  then  settles  to  an  average  of  the  two  distinct  array  tracks  throughout  the 
overlap  region.  Finally,  the  filtered  track  closely  follows  the  track  after  the  overlap  re¬ 
gion  has  ended  while  elfectively  removing  much  of  the  random  noise  present. 
Figure  21  on  page  29  is  the  smoothed  version  of  Figure  20  on  page  28,  and  proves  to 
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Figure  20.  Kalman  Filtered  Track  -  X  vs.  Y  (Arrays  1  &  11). 

be  an  easy-to-see  track  which  does  not  overshoot  the  target's  maneuver.  Thus,  the 
smoothing  routine  has  not  only  smoothed  the  filtered  track  considerably,  but  it  lias  also 
compensated  for  the  possible  tuning  problem.  Figure  22  on  page  30  is  the  smoothed 
track  superimposed  on  the  raw  data,  and  it  shows  this  clearly.  Figure  23  on  page  31  and 
Figure  24  on  page  32  show  the  filtered  and  smoothed  tracks  respectively  for  the  X  vs. 
Z  case.  These  plots  exhibit  the  same  characteristics  as  the  X  vs.  Y  plots  discussed  above 
as  expected.  Figure  25  on  page  33  highlights  the  overall  improvement  gained  by 
smoothing  the  raw  data. 

Another  set  of  data  which  contained  an  overlap  region  was  processed  by  the  pro¬ 
gram.  This  data  set  was  considerably  larger  than  the  previous  case  but  was  still  within 
the  capacity  of  the  personal  computer.  This  data  is  illustrated  in  Figure  26  on  page  34 
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Figure  21.  Smoothed  Trnck  -  X  vs.  Y  (Arrays  1  &  11). 


and  Figure  27  on  page  35.  Figure  28  on  page  36  displays  the  filtered  and  smoothed 
values  of  the  x  estimate  variances  vs.  time.  As  in  the  previous  case,  the  missed  samples 
a.  d  overlap  region  are  apparent  from  the  filtered  results,  and  the  smoothed  results  show 
muth  improvement.  Figure  29  on  page  37  shows  the  filtered  results  while  Figure  30  on 
page  ^  8  shows  the  smoothed  results  of  the  X  vs.  Y  tracks  for  the  arrays  2  &  12  overlap 
region,  and  it  is  clearly  illustrated  that  the  track  quality  is  again  vastly  improved. 
Figure  31  on  page  39  is  the  smoothed  X  vs.  Y  track  superimposed  on  the  raw  data 
demonstrating  that  the  smoothed  data  does  indeed  follow  the  raw  data  closely. 

>  Figure  32  on  page  40  and  Figure  33  on  page  41  show  the  filtered  and  smoothed  X  vs. 
Z  plots  respectively  with  equally  good  results.  Finally,  Figure  34  on  page  42  is  also  in- 

>  eluded  to  highlight  the  final  results  of  the  program.  Notice  that  there  is  no  overshoot 
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Figure  22.  Raw  &  Smoothed  Tracks  -  X  vs.  Y  (Arrays  1  &  1 1). 

in  these  filtered  tracks  as  was  seen  in  the  previous  case,  indicating  that  the  Kalman  Filter 
was  properly  tuned  for  this  run. 

These  results  clearly  demonstrate  that  the  Kalman  Filter  with  the  Fixed  Interval 
Smoothing  Algorithm  will  essentially  "solve”  the  array  handofF  problem  by  removing 
random  noise  and  taking  on  a  single  average  value  through  overlap  regions  based  on 
information  obtained  from  all  hydrophones  in  question.  I'hc  program  effectively  han¬ 
dles  missed  points  and  samples  with  multiple  data  especially  where  the  the  smoothed 
output  is  considered.  Therefore,  with  sufficient  computer  memory  available,  this  pro¬ 
gram  could  accept  a  target  s  entire  range  track  file  filled  with  missed  samples  and  over¬ 
lapping  data  regions  and  produce  the  optimum  track  for  the  known  informatioir.  1  his 
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Figure  23.  Kalman  Filtered  Track  -  X  vs.  Z  (Arrays  1  &  II). 


fact  makes  this  an  extremely  useful  post-data  analysis  tool  for  evaluating  typical  range 
ecnerated  tracks. 


31 


DEPTH  (FT) 


Figure  24.  Smoothed  Track  -  X  vs.  Z  (Arrays  1  &  11) 
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Figure  26.  Rau  Track  -  X  vs.  Y  (Arrays  2  &  12). 
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Figure  27.  Raw  Track  -  X  vs.  Z  (Arrays  2  &  12). 
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Figure  29.  Kalman  Filtered  Track  >  X  vs.  Y  (Arrays  2  &  12). 
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Figure  30.  Smoothed  Track  -  X  vs.  Y  (Arrays  2  &  12) 
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Figure  31.  Ran  &  Smoothed  Tracks  -  X  vs.  Y  (Arrays  2  &  12). 
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Figure  32.  Kalman  Filtered  Track  -  X  vs.  Z  (Arrays  2  &  12). 
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Figure  33.  Smoothed  Track  -  X  vs.  Z  (Arrays  2  &  12). 
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V.  CONCLUSION 


The  results  above  have  demonstrated  the  effectiveness  of  this  Kalman  Filter  Fixed 
Interval  Smoothing  Algorithm  in  smoothing  a  target  track  for  use  in  post-data  analysis. 
A  raw  target  track  characterized  by  discontinuities  in  array  overlap  regions  and  random 
noise  throughout  can  easily  be  transformed  into  the  optimal  track  given  the  known  in¬ 
formation.  All  the  user  need  do  is  set  up  an  appropriate  input  file  consisting  of  sample 
count  and  X,  Y,  and  Z  coordinates.  The  program  listed  in  Appendix  B  is  designed  to 
take  current  data  files  from  the  Keyport  Torpedo  Station  and  reformat  them  into  suit¬ 
able  input  files  for  the  main  smoothing  program  using  data  from  any  two  arrays.  This 
program  could  easily  be  modified  to  include  data  from  as  many  arrays  as  desired.  Thus, 
the  original  goal  of  this  research,  to  provide  a  smoothing  algorithm  tested  on  actual 
range  data  which  effectively  deals  with  the  array  handoff  problem,  has  been  successfully 
achieved.  In  addition,  this  algorithm's  success  suggests  some  other  interesting  possibil¬ 
ities  for  improving  short  base-line  array  tracking  range  capabilities. 

Another  possible  use  for  this  algorithm  is  range  calibration.  As  has  been  discussed, 
the  current  tracks  generated  by  the  short  base-line  arrays  contain  random  errors  and  bias 
enors  due  to  a  myriad  of  sources.  If  a  test  craft  could  be  tracked  accurately  independent 
of  the  acoustic  range,  it  could  be  used  to  generate  a  true  track  for  comparison  to  the 
smoothed  track  produced  by  this  program.  It  follows  that  since  the  Kalman  Filter 
Smoothing  routine  effectively  reduces  random  errors  while  having  no  effect  on  bias  er¬ 
rors,  any  bias  errors  present  in  the  smoothed  track  would  be  highlighted;  whereas,  they 
may  have  been  lost  before  in  the  random  noise.  These  bias  errors  could  then  be  ac¬ 
counted  for  in  the  software  that  calculates  the  target's  position.  Calibration  runs  could 
be  made  as  frequently  as  desired,  and  in  this  way,  bias  errors  could  be  promptly  and 
easily  compensated  for. 

The  Kalman  Filter  portion  of  the  program  presented  here  is  designed  to  operate  on 
a  file  of  existing  data  points.  However,  it  would  be  a  relatively  simple  matter  to  alter  this 
program  to  read  one  data  point,  operate  on  the  data,  generate  a  filtered  data  point,  and 
store  the  required  information  for  use  in  the  smoothing  portion  of  the  program.  The 
smoothing  routine  in  this  program  is  desired  to  run  after  all  the  data  has  been  Kalman 
filtered,  so,  no  changes  are  required  in  this  portion  of  the  program  as  long  as  the  last 
data  point  of  the  track  is  properly  flagged.  The  implications  of  these  facts  is  that  this 
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program  could  be  used  to  improve  the  real  time  tracks  generated  by  the  range  as  well 
as  providing  a  smoothed  track  after  the  run  is  complete  for  post-data  analysis  and  range 
calibration.  The  program  need  only  be  given  the  computed  output  of  the  tracking 
computer  and  then  allowed  to  supply  its  filtered  result  to  the  algorithm  w’hich  generates 
the  track  displays.  Furthermore,  improved  tracking  accuracy  could  be  obtained  using 
an  adaptive  excitation  matrix  scheme.  It  w’as  shown  earlier  how  Q(k)  can  affect  the  fil¬ 
tered  results.  Assuming  frequency  information  could  be  provided  to  the  program  in  real 
time,  the  magnitude  of  Q(k)  could  be  altered  in  response  to  doppler  shifts  in  received 
frequency.  In  other  words,  if  a  target  maneuver  is  indicated  by  a  doppler  shift,  the 
magnitude  of  Q(k)  is  increased  to  allow  the  filter  to  track  through  the  maneuver.  Once 
the  doppler  information  indicates  that  the  target  is  no  longer  maneuvering,  Q(k)  is  de¬ 
creased  to  smooth  out  the  filtered  track.  In  this  way,  the  current  real  time  tracking  ca¬ 
pabilities  of  these  ranges  could  be  improved  with  a  reasonable  amount  of  changes  in  the 
tracking  softw'are  required. 

In  conclusion,  the  approach  used  here  of  Kalman  Filtering  the  target's  computed 
position  in  X,  Y,  and  Z  coordinates  is  considered  to  be  successful.  This  method  is  an 
effective  compromise  between  the  theory’  of  optimal  linear  estimation  and  the  opera¬ 
tionally  oriented  user  who  may  be  leery  of  trusting  massaged  data,  because  the  raw’  data 
is  not  lost.  Therefore,  the  user  can  ahvays  make  comparisons  betw’een  raw'  and  treated 
data.  In  addition,  this  program  is  seen  as  flexible  enough  to  be  applied  to  a  number  of 
possible  applications  concerning  the  existing  short  base-line  array  tracking  ranges. 
Finally,  it  is  an  excellent  post-data  analysis  tool  in  its  present  form. 
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APPENDIX  A.  KALMAN  FILTER  FIXED  INTERVAL  SMOOTHING 

ALGORITHM 


A.  MAIN  PROGRAM 

A  brief  description  of  the  main  program  and  subroutines  is  given.  Initially,  the 
program  reads  in  data  from  a  user- specified  data  file  and  sets  a  point  counter  represented 
by  the  variable  IRl.  After  initialization  of  the  necessary  variables,  the  program  deter¬ 
mines  the  time  slot  to  the  next  sequential  data  point  in  the  file  and  converts  it  to  seconds 
for  use  by  the  subroutine  which  calculates  the  state  transition  matrix  0.  Next,  the 
excitation  matrix  Q(k)  is  computed.  Notice  that  this  calculation  requires  the  time 
elapsed  from  the  last  sample.  The  gain  matrix  G{k)  is  then  computed  followed  by  the 
filtered  estimates  of  the  states.  The  program  then  stores  the  filtered  estimates  along  with 
the  information  needed  for  the  smoothing  algorithm,  and  then  initializes  the  array  for 
later  storage  of  the  smoothed  estimates.  The  variance  of  the  x  estimate  is  sent  to  a 
subroutine  to  be  placed  in  a  data  file. 

The  Fixed  Interval  Smoothing  portion  of  the  program  first  sets  up  its  loop  counter 
to  count  back  from  the  filtered  loop  endpoint.  Next,  4>  is  recomputed  using  times  be¬ 
tween  samples  that  are  consistent  with  the  Kalman  Filter  case.  Pertinent  information 
stored  in  the  Kalman  Filter  loop  is  recovered,  and  the  smoothed  estimates  are  then 
solved  for.  The  smoothed  x  estimate  variance  is  then  sent  to  a  subroutine  which  includes 
it  in  an  output  file,  and  finally,  the  filtered  and  smoothed  estimates  are  witten  to  output 
data  files. 

B.  SUBROUTINES 

•  PLOTPKKS 

outputs  the  variance  of  the  x  estimate  vs.  discrete  time  interval  for  the  Kalman 
Filtered  case  and  the  Smoothed  case 

•  PHIDEL 

computes  the  state  transition  matrix  based  on  time  between  samples  using  a 
linear  second  order  model 

•  GAIN 

computes  P(k/k-l),P(k;k),  and  the  gain  matrix  G(k) 

•  ADD 

adds  two  input  matrices 

•  SUB 

subtracts  second  input  matrix  from  first 
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•  PROD 

multiplies  two  input  matrices 

•  TRANS 

calculates  the  transpose  of  the  matrix  input 

•  RECIP 

algorithm  taken  from  [Ref.  17:  p.  163]  which  inverts  the  input  matrix 

•  INITS 

initializes  matrices  at  the  beginning  of  the  main  program 

•  CHANGE 

allows  user  to  change  the  magnitude  of  the  excitation  matrix  Q(k)  and  the  ini¬ 
tial  value  of  the  estimate's  covariance  of  error  matrix  P{k/k) 


$ LARGE 

C---KALMAN  FILTER- -"KALMAN  SMOOTHING  ALGORITHM . 

C---THIS  PROGRAM  IS  DESIGNED  TO  PROCESS  3/D  DATA  FILES  FROM 
C---THE  UNDERSEA  TRACKING  RANGE  AT  KEYPORT,  WA.  A  KALMAN  FILTER 
C---IS  APPLIED  TO  THE  TRACK  DATA  WHICH  CONSISTS  OF  X.Y,  AND  Z 
C---COORDINATES.  THEN,  A  KALMAN  FILTER  SMOOTHING  ROUTINE  GENERATES 
C---SMOOTHED  POINTS  IN  X.Y,  AND  Z.  THE  PROGRAM  GENERATES  OUTPUT 
C---FILES  WHICH  CONTAIN  THE  VARIANCE  OF  THE  X  ESTIMATE  VS  DISCRETE  TIME 
C---FOR  BOTH  THE  FORWARD  KALMAN  FILTER  CASE  AND  THE  KALMAN  SMOO'THED 
C---CASE.  FILES  ARE  ALSO  GENERATED  WHICH  CONTAIN  THE  FILTERED 
C---X,Y,AND  Z  ESTIMATES  AND  THE  SMOOTHED  X,Y,AND  Z  ESTIMATES. 

C---THIS  PROGRAM  IS  DESIGNED  TO  RUN  ON  THE  IBM/ AT  PERSONAL 
C-- -COMPUTER  BUT  DUE  TO  THE  SIZE  OF  THE  DATA  SETS  INVOLVED,  PLOTTING 
C---CANNOT  BE  DONE  WITH  THIS  PROGRAM.  PLOTTING  OF  OUTPUT  DATA  IS 
C---DONE  USING  MATLAB,  THE  PROGRAM  GIVES  THE  USER  THE  OPTION  OF 
C-- -CHANGING  THE  VALUE  OF  THE  INITIAL  COVARIANCE  MATRIX  AND  THE 
C-- -EXCITATION  PROCESS  VECTOR.  THE  USER  IS  ALSO  REQUIRED  TO  PROVIDE 
C---THE  NAMES  OF  THE  INPUT  AND  OUTPUT  DATA  FILES. 

C 

c  *****  declaration  of  variables  ***** 

C 

COMMON  W(6,6),XKK(6),PHI(6,6),Q(6,6),XKKM1(6), 

*  A(6,6),B(6,6),H(6,6),HI(6,6),PKKM1(6,6),PKK(6,6) 

COMMON  /CBLK/  RAW(3, 1000) ,FILT(3, 1000) ,SMOOTH(3, 1000) ,PTC( 1000) , 

*  XYRANGE(3,2),PKKONEONE(1000),IR1 
CHARACTER  INFIL*13,  FILTER*13,  OUTFIL*13 

DIMENSION  R( 6 , 6) ,G( 6 . 6) ,XRR( 1000) , YRR( 1000) , ZRR( 1000) ,PHIT( 6,6), 
*ZZ(6),E(6),GE(6),PKKS3D(6,6,1000),PKKM1S(6,6,1000),XKKS(6,1000), 
*C(2,2),D(2,2),PKKS(6,6),P2(6,6),PNNM1(6,6),ZKKM1(6),AKT(6,6), 
*SS3(6,6),SS3R(6,6),AK(6,6),XNNM1(6),SS2(6),XP1(6),TEMP4(6,6), 
*TEMP5( 6 , 6) ,TEMP6( 6, 6) ,TEMP3( 6, 6) ,TEMP2( 6) ,TEMP1( 6 , 6) ,TMP( 6) 

****  INPUT  DATA,  DESIGNATE  FILENAMES  **** 

WRITE(*  *)  'ENTER  NAME  OF  INPUT  DATA  FILE’ 

READ(*,'(A)')  INFIL 

WRITE(*  *)  'ENTER  NAME  OF  FILTERED  DATA  FILE' 

READ(*,*(A)')  FILTER 

WRITE(*  *)  'ENTER  NAME  OF  SMOOTHED  DATA  FILE’ 

READ(*,HA)')  OUTFIL 
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0PEN(1,FILE=  INFIL,STATUS=  'OLD') 

0PEN(2,FILE=  FILTER, STATUS=  'NEW') 

0PEN(3,FILE=  OUTFIL,STATUS=  'NEW') 

IR1=1 

15  READ( 1,*,END=20)PTC( IRl) ,XRR(IR1) ,YRR( IRl) ,ZRR( IRl) 
IR1=IR1+1 
GOTO  15 
20  CONTINUE 

IRl  =  IRl  -  1 

****  INITIALIZE  VARIABLES  AND  DATA  ARRAYS  **** 

L  =  1 
M  =  3 
N  =  6 
MT  =  6 
ND  =  6 
LD  =  6 
DT  =  0. 

K  =  0 

W(l,l)  =  .0001 
R(l,l)  =  25 
R(2,2)  =  25 
R(3,3)  =  25 
R(4,4)  =  25 
R(5,5)  =  25 
R(6,6)  =  25 
CALL  INITS 

****  CHECK  INITIAL  PARAMETERS  **** 

CALL  CHANGE (W.PKKMl) 


*Vr*iSr***TSrV<**yf*****yf************ 

KALMAN  FILTER  ROUTINE 

fr**********************^^****** 


100  K  =  K  +  1 

GENERATE  PHI  AND  Q  MATRICES  **** 

IF  (K  .EQ.  IRl)  THEN 
DT  =  2.  666 
ELSE 

DT  =  1. 333*(PTC(K+1)  -  PTC(K)) 

END  IF 

CALL  PHIDEL(DT,N,A,PHI) 

IF  (K  .EQ.  1)  THEN 
DT  =  2.  666 
ELSE 

DT  =  1.  333*(PTC(K)-  PTC(K-l)) 

END  IF 

Q(l,l)  =  (DT»>*4/4)  *  W(l,l) 

Q(2,2)  =  DT**2  *  W(l,l) 

Q(3,3)  =  Q(l,l) 

Q(^,4)  =  Q(2,2) 
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Q(5,5)  =  Q(l,l) 

Q(6,6)  =  Q(2,2) 

RAW(1,K)  =  XRR(K) 

RAW(2,K)  =  YRR(K) 

RAW(3,K)  =  ZRR(K) 

****  CALCULATE  GAIN  MATRIX  AND  SOLVE  FOR  **** 

****  X(K/K)  =  X(K/K-1)  +  G(K)*[Z(K)  -  ZCK/K-1)]  **** 

****  WHERE  Z(K)  IS  THE  OBSERVABLE  **** 

CALL  GAIN(PKK,PKKM1,Q,R,PHI,H,N,G,HI,ND,MD,LD) 

CALL  PR0D(PHI,XKK,N,N,1,XKKM1,ND,MD,LD) 

CALL  PR0D(H,XKKM1,N,N,1,ZKKM1,ND,MD,MD) 

ZZ(1)=XRR(K) 

ZZ(2)=YRR(K) 

ZZ(3)=ZRR(K) 

CALL  SUB(ZZ,ZKKM1,M,1,E,ND,MD) 

CALL  PR0D(G,E,N,M,1,GE,ND,MD,LD) 

CALL  ADD(XKKM1,GE,N,1,XKK,ND,MD) 

****  STORE  X(K/K),P(K/K),P(K/K-1), INITIALIZE  THE  SMOOTHED 
****  data  array,  and  STORE  THE  FILTERED  ESTIMATES. 

DO  40  I  =  1,6 
DO  40  J  =  1,6 
PKKS3D(I,J,K)  =  PKK(I,J) 

40  PKKM1S(I,J,K)  =  PKKM1(I,J) 

DO  50  1=1,6 

50  XKKS(I,K)  =  XKK(I) 

DO  60  I  =  1,3 

FILT(I,K)  =  XKK( 2*1-1) 

60  SM00TH(I,K)  =  FILT(I,K) 

PKKONEONE(K)  =  PKKS3D( 1,1,K) 

IF  (K.  LT.  IRl)  GOTO  100 
CT  =  1 

CALL  PLOTPKKS(CT) 

************Vf**':'r****Vr*'****ifr***** 

KALMAN  SMOOTHING  ROUTINE 

***VfVf**'********'>V'*yf  **•******■*■*■**** 


DO  600  K=1,IR1  -  1 
KI=  IRl  -  K 

****  generate  phi  matrix  and  retrieve  STORED  X(K/K),  **** 

****  P(K,K),  AND  P(K+1/K)  **** 

DT  =  1. 333*(PTC(KI+1)  -  PTC(KI)) 

CALL  PHIDEL(DT,N,A,PHI) 

DO  501  I  =  1,6 

XPl(I)  =  XKKS(I,KI) 

501  CONTINUE 

DO  502  I  =  1,6 
DO  502  J  =  1,6 

P2(I,J)  =  PKKS3D(I,J,KI) 

SS3(I,J)  =  PKKM1S(I,J,KI) 


**** 

**** 
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502  CONTINUE 

****  SOLVE  FOR  SMOOTHED  ESTIMATE  **** 

****  X(K/N)  =  X(K/K)  +  A(K)*[X(K+1/N)  -  X(K+1/K)]  **** 

CALL  TRANS(PHI,N,N,PHIT,ND,MD) 

CALL  RECIP(SS3,SS3R,N) 

CALL  PR0D(PHIT,SS3R,N,N,N,TEMP1,ND,MD,LD) 

CALL  PR0D(P2, TEMPI, N,N,N,AK,ND,MD,MD) 

DO  504  I  =  1,6 
XNNMl(I)  =  XKKS(I,KI+1) 

504  CONTINUE 

CALL  PR0D(PHI,XP1,N,N,L,SS2,ND,MD,L) 

CALL  SUB(XNNM1,SS2,N,1,TEMP2,ND,L) 

CALL  PR0D(AK,TEMP2,N,N,1,TEMP3,ND,MD,L) 

CALL  ADD(XP1,TEMP3,N,1,TMP,ND,MD) 

DO  505  I  =  1,6 

505  XKKS(I,KI)  =  TMPC  I) 

DO  506  I  =  1,6 

DO  506  J  =  1,6 

PNNM1(I,J)  =  PKKS3D(I,J,KI+1) 

506  CONTINUE 

CALL  SUB(PNNM1,SS3,N,6,TEMP4,ND,MD) 

CALL  TRANS(AK,N,N,AKT,ND,MD) 

CALL  PR0D(TEMP4,AKT,N,N,N, TEMPS, ND,MD,MD) 

CALL  PR0D(AK,TEMP5,N,N,6,TEMP6,ND,MD,MD) 

CALL  ADD(P2,TEMP6,N,6,PKKS,ND,MD) 

DO  508  I  =  1,6 
DO  508  J  =  1,6 

508  PKKS3D(I,J,KI)  =  PKKS(I,J) 

****  STORE  X(K/K),P(K/K),AND  OUTPUT  SMOOTHED  DATA  **** 
DO  520  I  =  1,3 

520  SMOOTH(I,KI)  =  XKKS(2*I-1,KI) 

PKKONEONE(KI)  =  PKKS3D( 1 , 1 ,KI) 

600  CONTINUE 
CT  =  0 

CALL  PLOTPKKS(CT) 

DO  650  K  =  1,IR1 

WRITEC 2 ,*) (FILTC I ,K) , 1=1 , 3) 
TOITE(3,*)(SMOOTH(I,K),I=l,3) 

650  CONTINUE 
END 


SUBPROUTINE  WHICH  OUTPUTS  THE  VARIANCE  OF  THE  X  ESTIMATE  VS 
DISCRETE  TIME  FOR  BOTH  THE  FILTERED  CASE  AND  THE  SMOOTHED  CASE 


SUBROUTINE  PLOTPKKS(CT) 

COMMON  /CBLK/  RAW( 3 , 1000) ,FILT(3 , 1000) ,SMOOTH(3, 1000) ,PTC( 1000) , 
*  XYRANGE(3,2),PKKONEONE(lO0O),IRl 
DIMENSION  TK(IOOO) 

CHARACTER  FWDVAR3*13,  BKVAR*13 
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IF  (CT  .EQ.  1)  THEN 

WRITE (**)  'ENTER  NAME  OF  FILTERED  VARIANCE  FILE' 

READ(*/(A)')  FWDVAR 

0PEN(4,FILE=  FWDVAR,  STATUS=  'NEW') 

ELSE 

WRITE(* ,*)  'ENTER  NAME  OF  SMOOTHED  VARIANCE  FILE' 

READ(*,'(A)')  BKVAR 

OPEN(5,FILE=  BKVAR,  STATUS*  'NEW') 

END  IF 

DO  10  1=1, IRl 

TK(I)=PTC(I)-PTC(1) 

IF  (CT  .EQ.  1)  THEN 
WRITEC 4 , *)TK( I ) , PKKONEONEC I ) 

ELSE 

WRITEC 5 , *)TK( I ), PKKONEONEC I ) 

END  IF 
10  CONTINUE 
RETURN 
END 

********************************************* 
SUBROUTINE  WHICH  COMPUTES  THE  PHI  MATRIX 
PHI  =  I  +  A*T 

********************************************** 


SUBROUTINE  PHIDEL(T,N,A,PHI) 

DIMENSION  A(6,6) ,PHI(6,6) ,COR(6,6) ,C(6,6) 
F=l. 

DO  100  IR  =  1,N 
DO  100  IC  =  1,N 
PHI(IR,IC)  =  0. 

PHI(IR,IR)  =  1. 

100  CCIR.IC)  =  A(IR,IC) 

110  DO  120  IR  =  1,N 
DO  120  IC  =  1,N 
COR(IR,IC)  =  T/F*C(IR,IC) 

120  PHI(IR,IC)  =  PHI(IR,IC)+COR(IR,IC) 
RETURN 
END 


THIS  SUBROUTINE  COMPUTES  THE  OPTIMUM  GAIN  MATRIX  G(K)  AND 
THE  COVARIANCE  MATRIX  P(K+1/K)  BASED  ON  THE  EQUATIONS: 

G(K)  =  P(K/K-1)*TRANS'’H(K)  *INV®I H(K)*P(K/K-1)*TRANS'’HCK)  +  R] 

P(K/K+1)  =  PHI*P(K/K)*TRANS®PHI  +  Q(K) 


SUBROUTINE  GAIN(PKK,PKKM1,Q,R,PHI,H,N,G,HI,ND,MD,LD) 
DIMENSION  PKK(6,6),Q(6,6),H(6,6),G(6,6),R(6,6),C(6,6),D(6,6), 

*  HI(6,6),HT(6,6),TEMP(6,6),TEMP1(6,6),TEMP2(6,6), 

*  PHI(6,6),PHIT(6,6),PKKM1(6,6),TEMP3(6,6),TEMP4(6,6) 

CALL  TRANS(H,N,N,HT,ND,MD) 

CALL  PR0D(PKKM1,HT,N,N,N,TEMP,ND,MD,LD) 

CALL  PRODCH, TEMP, N,N,N, TEMPI, ND,MD,LD) 
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DO  140  1=1, N 
DO  140  J=1,N 

140  TEMP3(I,J)=TEMP1(I,J) 

TEMP3(1,1)=TEMP1(1,1)+R(1,1) 

TEMP3( 2 . 2 )=TEMP1 ( 2 , 2 )+R( 2 , 2) 

TEMP3( 3 , 3)=TEMP1( 3 , 3)+R( 3 , 3) 
TEMP3(4,4)=TEMP1(4,4)+R(4,4) 
TEMP3(5,5)=TEMP1(5,5)+R(S,5) 
TEMP3(6,6)=TEMP1(6,6)+R(6,6) 

DO  143  1=1, N 
DO  143  J=1,N 

143  C(I,J)=TEMP3(I,J) 

CALL  RECIP(C,D,N) 

170  CALL  PROD(TEMP,D,N,N,N,G,ND,MD,LD) 

CALL  PROD(G,H,N,N,N,TEMP,ND,MD,LD) 

DO  180  I  =  1,6 
DO  180  J  =  1,6 

180  TEMP(I,J)  =  -TEMP(I,J) 

CALL  ADD(HI, TEMP, N,N, TEMP, ND,MD) 

CALL  PR0D(TEMP,PKKM1,N,N,N,PKK,ND,MD,LD) 
CALL  TRANS(PHI,N,N,PHIT,ND,MD) 

CALL  PROD(PKK,PHIT,N,N,N,TEMP,ND,MD,LD) 
CALL  PR0D(PHI, TEMP, N,N,N, TEMPI, ND,MD,LD) 
CALL  ADD(TEMP1,Q,N,N,PKKM1,ND,MD) 

RETURN 

END 


****** 

SUBROUTINE  WHICH  ADDS  TWO  INPUT  MATRICES 
**************************  ************^ 


SUBROUTINE  ADD(A,B,N,M,C,ND,MD) 
DIMENSION  A(ND,MD),B(ND,MD),C(ND,MD) 
DO  100  I  =  1,N 
DO  100  J  =  1,M 

100  C(I,J)  =  A(I,J)  +  B(I,J) 

RETURN 

END 


**************'**Vc******'*****Vf************************************** 


SUBROUTINE  WHICH  SUBTRACTS  THE  SECOND  INPUT  MATRIX  FROM  THE  FIRST 


frVfVfV 


SUBROUTINE  SUB(A,B,N,M,C,ND,MD) 
DIMENSION  A(ND,MD),B(ND,MD),C(ND,MD) 
DO  100  I  =  1,N 
DO  100  J  =  1,M 

100  C(I,J)  =  A(I,J)  -  B(I,J) 

RETURN 

END 


SUBROUTINE  WHICH  MULTIPLIES  TWO  INPUT  MATRICES 


SUBROUTINE  PROD(A,B,N,M,L,C,ND,MD,LD) 
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n  o  o  Cl  Cl  Cl  o  Cl  Cl  Cl 


DIMENSION  A(ND,MD),B(MD.LD),C(ND,LD) 
DO  100  I  =  1,N 
DO  100  J  =  1,L 
100  C(I,J)  =  0. 

DO  110  I  =  1,N 
DO  110  J  =  l.L 
DO  110  K  =  1,M 

110  C(I,J)  =C(I.J)  +  A(I,K)*B(K,J) 
RETURN 
END 


SUBROUTINE  WHICH  COMPUTES  THE  TRANSPOSE  OF  THE  INPUT  MATRIX 


SUBROUTINE  TRANS(A,N,M,C,ND,MD) 
DIMENSION  A(ND,MD),C(MD,ND) 

DO  100  I  =  1,N 
DO  100  J  =  1,M 
100  C(J,I)  =  A(I,J) 

RETURN 

END 


SUBROUTINE  WHICH  CALCULATES  THE  RECIPROCAL  OF  THE  INPUT  MATRIX 


SUBROUTINE  RECIP(A,C,N) 

DIMENSION  A(N,N),C(N,N),D(20,20) 

DO  100  I  =  1,N 
DO  100  J  =  1,N 
100  D(I,J)  =  A(I,J) 

DO  115  I  =  1,N 
DO  115  J  =  N+1,2*N 
115  D(I,J)  =0.0 

DO  120  I  =  1,N 
J  =  I  +  N 
120  D(I,J)  =  1.0 
DO  240  K  =  1,N 
M  =  K  +  1 

IF  (K  .EQ.  N)  GOTO  180 
L  =  K 

DO  140  I  =  M,N 

140  IF  (ABS(D(I,K))  . GT.  ABSfD(L,K)))  L  =  I 
IF  (L  .EQ.  K)  GOTO  180 
DO  160  J  =  K,2*N 
TEMP  =  D(K,J) 

D(K,J)  =  D(L,J) 

160  D(L,J)  =  TEMP 

180  DO  185  J  =  M,2*N 

185  D(K,J)  =  D(K,J)/D(K,K) 

IF  (K  .EQ.  1)  GOTO  220 
Ml  =  K  -  1 
DO  200  I  =  1,M1 
DO  200  J  =  M,2*N 

200  D(I,J)  =  D(I,J)  -  D(I,K)*D(K,J) 
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o  o  o  o  n 


IF  (K  .EQ.  N)  GOTO  260 
220  DO  240  I  =M,N 

DO  240  J  =  M,2*N 

240  D(I,J)  =  D(I,J)  -  D(I,K)*D(K,J) 
260  DO  265  I  =  1,N 
DO  265  J  =  1,N 
K  =  J  +  N 

265  C(I,J)  =  D(I,K) 

RETURN 

END 


SUBROUTINE  WHICH  INITIALIZES  SEVERAL  MATRICES 


SUBROUTINE  INITS 

COMMON  W(6,6),XKK(6),PHI(6,6),Q(6,6),XKKM1(6), 

*  A(6,6),  B(6,6),  H(6,6),  HI(6,6),  PKKM1(6,6) ,PKK(6,6) 
DO  190  I  =  1,  6 
XKK(I)  =  0. 

DO  190  J  =  1,  6 

Q(I,J)  =  0. 

PHI(I,J)  =  0. 

A(I,J)  =  0. 

B(I,J)  =  0. 

H(I,J)  =  0. 

HI(I,J)  =  0. 

Hid, I)  =  1. 

PKK(I,J)  =  0. 

PKK(I,I)  =  1. 

PKKM1(I,J)=0.  0 
PKKM1(I,I)  =  1000000.0 
190  CONTINUE 
A(l,2)  =  1. 

A(3,4)  =  1. 

A(5,6)  =  1. 

H(l,l)  =  1. 

H(2,3)  =  1. 

H(3,5)  =  1. 

RETURN 

END 


C 

C 

C 

C 

C 

C 


SUBROUTINE  WHICH  ALLOWS  CHANGING  OF  W  AND  PKKMl  TO  ALTER  THE 
BEHAVIOR  OF  THE  FILTER  WITHOUT  HAVING  TO  RE-COMPILE  THE  PROGRAM 


SUBROUTINE  CHANGE (W, PKKMl) 

CHARACTER*!  ASK 
DIMENSION  W(6,6),PKKM1(6,6) 

REAL*4  X 
5  FORMAT(Al) 

WRITE(*,10)W(1,1) 

10  FORMATC'  W(l,l)  IS  THIS  VALUE:  ' ,F11. 5) 
WRITE(*.20) 

20  FORMATC^  ENTER  (Y)  TO  CHANGE  IT.  ') 
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.OR.  ASK  .EQ.  'y')  THEN 


READ(*,5)ASK 
X=W(1,1) 

IF  (ASK  .EQ.  'Y' 

WRITE(*,40) 

READ(*,50)X 
ENDIF 

DO  25  1=1,6 
W(I,I)=X 
25  CONTINUE 

WRITE(*,30)PKKM1(1,1) 

30  FORMATC'  PKKM1(I,I)  IS  THIS  VALUE:  ’ ,F10. 4) 

WRITE(*,20) 

READ(*,5)ASK 

X=PKKM1(1,1) 

IF  (ASK  .EQ.  'Y'  .OR.  ASK  .EQ.  ’y’)  THEN 
WRITE(*,40) 

READ(*,50)X 

ENDIF 

DO  35  1=1,6 
PKKM1(I,I)=X 
35  CONTINUE 

40  FORMATC  ENTER  A  VALUE  FOR  ELEMENTS(I,I)  WITH  A  DECIMAL  POINT.  ') 
50  FORMAT(G10.  5) 

RETURN 

END 


APPENDIX  B.  INPUT  DATA  FILE  FORMATTING  PROGRAM 

This  program  is  designed  to  set  up  properly  formatted  files  for  use  as  input  data  by 
the  main  program  shown  in  Appendix  A.  It  is  an  abbreviated  version  of  a  program 
presented  in  [Ref  18]  which  w'as  developed  to  create  usable  data  files. 

The  program  first  reads  in  the  names  of  the  desired  input  and  output  files  and  the 
array  numbers  of  interest.  An  option  is  given  the  user  to  remove  unwanted  headers  from 
the  input  data  file,  which  some  of  the  available  files  have,  then  the  target  of  interest  is 
requested.  Finally,  the  program  writes  to  the  output  file  only  those  data  points  that 
meet  the  specified  criteria  or  issues  an  error  message  if  appropriate. 


Q  *********Vf***************Vc**Vf***********ifr*******Vf************* 

C  PROGRAM  TO  READ  IN  RAW  DATA  FROM  KEYPORT  HYDROPHONE  ARRAYS, 

C  SEGREGATE  IT  BY  USER  SPECIFIED  MODE,  AND  RETAIN  DATA  ONLY 

C  FROM  THE  HYDROPHONES  SELECTED.  INPUT  AND  OUTPUT  FILE  NAMES 

C  ARE  PROVIDED  BY  THE  USER. 

C  ****Vf**?V**************************************iSnHr**iJr**iHf****** 

C 

CHARACTER  D0*2,  DSNAME*13,  0UTFIL*13 
CHARACTER*3  ENDCHK 
CHARACTER*!  C 

INTEGER  PC,  ARRAY,  NHEAD,A1,A2,NUM 

WRITE(* ,*)  '  ENTER  THE  NAME  OF  YOUR  INPUT  FILE:  ’ 

READ(*,'(A)')  DSNAME 

WRITE(*,*)  'INPUT  NAME  OF  DESIRED  OUTPUT  FILE’ 

READ(*,'(A)')  OUTFIL 

WRITE (*,*)  'ENTER  NUMBERS  OF  ARRAYS  TO  BE  PAIRED' 

READ(*  *)  A1,A2 

OPEN( 1 ’fILE=DSNAME , STATUS=' OLD’ ) 
0PEN(2,FILE=0UTFIL,STATUS='NEW' ) 

WRITEC*,*)'  DO  YOU  WANT  TO  REMOVE  HEADERS?  (Y  or  N) ’ 
READ(*,’(A)')  C 
IFCC.EQ. 'N')  GOTO  12 

11  READ(1,120)ENDCHK 
WRITEC*,*)  ENDCHK 
IFCENDCHK.NE.  'END' )  GOTO  11 

12  CONTINUE 

WRITEC*,*)'  INPUT  MODE  TO  BE  KEPT?' 

READC*,*)  NUM 


10  READC 1, 100, END=50)PC, DO, X,Y,Z,ARRAY, MODE 
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IF(DO  .EQ.  'EV')GOTO  20 
IFCMODE  .ME.  NUM)  GOTO  20 

IF(( ARRAY  .NE.  Al)  .AND.  (ARRAY  .NE.  A2))  GO  TO  20 
WRITE(2,110)PC,X,Y,Z, ARRAY, MODE 
20  CONTINUE 
GOTO  10 


WRITEC*,*)’  THERE  WAS  AN  ERROR  IN  THE  FILE' 

50  CONTINUE 

100  F0RMAT(I5,A2,1X,F7.  1,2X,F7.  1,2X,F7.  1,30X,I2,1X,I2) 

110  FORMAT(1X,I5,2X,3F10.  1,2X,I2,2X,I2) 

120  FORMAT(31X,A3) 

CLOSE  (UNIT=1) 

CLOSE(UNIT=2) 

END 
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